#include "../godot_headers/gdnative_api_struct.gen.h"
#include <unistd.h>
#include <asm/unistd.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <modbus/modbus.h>
#include <pthread.h>

#include <sys/types.h>
#include <sys/syscall.h>

const godot_gdnative_core_api_struct *api = NULL;
const godot_gdnative_ext_nativescript_api_struct *nativescript_api = NULL;

void *modbus_constructor(godot_object *p_instance, void *p_method_data);
void modbus_destructor(godot_object *p_instance, void *p_method_data, void *p_user_data);
godot_variant modbus_get_data(godot_object *p_instance, void *p_method_data,
		void *p_user_data, int p_num_args, godot_variant **p_args);

typedef struct user_data_struct {
	modbus_t *ctx;
	modbus_mapping_t *mb_mapping;
} user_data_struct;

user_data_struct ud;

//typedef struct server_parameters {
//	int port;
//	char *address;
//} server_parameters;

void GDN_EXPORT godot_gdnative_init(godot_gdnative_init_options *p_options) 
{
	api = p_options->api_struct;

	// Now find our extensions.
	for (int i = 0; i < api->num_extensions; i++) {
		switch (api->extensions[i]->type) {
			case GDNATIVE_EXT_NATIVESCRIPT: {
								nativescript_api = (godot_gdnative_ext_nativescript_api_struct *)api->extensions[i];
							}; break;
			default: break;
		}
	}
}


void GDN_EXPORT godot_gdnative_terminate(godot_gdnative_terminate_options *p_options) 
{
	api = NULL;
	nativescript_api = NULL;
}
void *server_routine(void *user_data_void_p)
{
	user_data_struct *user_data = (user_data_struct *) user_data_void_p;
	//printf("in server_routine address is %s and port is %d\n", user_data->ctx->address, user_data->ctx->port);
	int s = -1;
	for (;;) {

		s = modbus_tcp_listen(user_data->ctx, 1);
		printf("before accept\n");
		printf("[%d]: outer loop \n", syscall(__NR_gettid));
		modbus_tcp_accept(user_data->ctx, &s);

		for (;;) {
			uint8_t query[MODBUS_TCP_MAX_ADU_LENGTH];
			int rc;

			printf("[%d]: inner loop entry\n", syscall(__NR_gettid));
			rc = modbus_receive(user_data->ctx, query);
			if (rc > 0) {
				/* rc is the query size */
				modbus_reply(user_data->ctx, query, rc, user_data->mb_mapping);
			} else if (rc == -1) {
				/* Connection closed by the client or error */
				break;
			}
		}

		printf("Quit the loop: %s\n", modbus_strerror(errno));

		if (s != -1) {
			close(s);
		}
	}
	modbus_mapping_free(user_data->mb_mapping);
	modbus_close(user_data->ctx);
	modbus_free(user_data->ctx);
}

void *modbus_constructor(godot_object *p_instance, void *p_method_data) 
{
	user_data_struct *user_data = api->godot_alloc(sizeof(user_data_struct));
	// init is delayed so that we can pass arguments normally
	return user_data;
}

void modbus_destructor(godot_object *p_instance, void *p_method_data, void *p_user_data) 
{
	api->godot_free(p_user_data);
}

godot_variant modbus_get_holding_register(godot_object *p_instance, void *p_method_data,
		void *p_user_data, int p_num_args, godot_variant **p_args) 
{
	godot_variant ret;
	user_data_struct *user_data = (user_data_struct *)p_user_data;
	int reg_num = 0;

	int value = user_data->mb_mapping->tab_registers[reg_num];
	api->godot_variant_new_int(&ret, value);
	return ret;
}

godot_variant modbus_set_holding_register(godot_object *p_instance, void *p_method_data,
		void *p_user_data, int p_num_args, godot_variant **p_args) 
{
	godot_variant ret;
	user_data_struct *user_data = (user_data_struct *)p_user_data;
	int reg_num = 0;
	godot_variant *gd_value = p_args[0];

	int value = api->godot_variant_as_int(gd_value);
	user_data->mb_mapping->tab_registers[reg_num]=value;
	return ret;
}

godot_variant modbus_set_coil(godot_object *p_instance, void *p_method_data,
		void *p_user_data, int p_num_args, godot_variant **p_args) 
{
	godot_variant ret;
	user_data_struct *user_data = (user_data_struct *)p_user_data;
	int reg_num = 0;
	godot_variant *gd_value = p_args[0];

	bool value = api->godot_variant_as_bool(gd_value);
	modbus_set_bits_from_byte(&user_data->mb_mapping->tab_bits[0], reg_num, (uint8_t) value);

	return ret;
}


godot_variant modbus_get_coil(godot_object *p_instance, void *p_method_data,
		void *p_user_data, int p_num_args, godot_variant **p_args) 
{
	godot_variant ret;
	user_data_struct *user_data = (user_data_struct *)p_user_data;
	int reg_num = 0;
	int bit_idx = 0;

	uint8_t value = user_data->mb_mapping->tab_bits[reg_num];
	printf("get coil native: %d\n", value);
	value = ((value >> bit_idx)  & 0x01);
	api -> godot_variant_new_bool(&ret, (godot_bool) value); //godot_bool is just a typedefed bool
	return ret;
}

godot_variant modbus_start_server(godot_object *p_instance, void *p_method_data,
		void *p_user_data, int p_num_args, godot_variant **p_args) 
{
	//server_parameters srv_parameters;
	pthread_t server_thread;
	//pthread_attr_t attr;
	int port;
	const char* address;
	godot_variant ret;

	user_data_struct *user_data = (user_data_struct *)p_user_data;
	
	//pthread_attr_init(&attr);

	// start_server(address,port)
	godot_variant *gd_variant_address = p_args[0];
	godot_variant *gd_variant_port = p_args[1];
	
	port = api->godot_variant_as_int(gd_variant_port);
	godot_string gd_string_address = api->godot_variant_as_string(gd_variant_address);
	godot_char_string gd_string_ascii_address = api->godot_string_ascii(&gd_string_address);
	address = api->godot_char_string_get_data(&gd_string_ascii_address);

	user_data->ctx = modbus_new_tcp(address, port);


	printf("created new context with address %s and port %d\n", address, port);

	user_data->mb_mapping = modbus_mapping_new(500, 500, 500, 500);
	if (user_data->mb_mapping == NULL) {
		fprintf(stderr, "Failed to allocate the mapping: %s\n",
				modbus_strerror(errno));
		modbus_free(user_data->ctx);
		return ret;
	}

	pthread_create(&server_thread, NULL, &server_routine, user_data);

	return ret;
}

void GDN_EXPORT godot_nativescript_init(void *p_handle) 
{
	godot_instance_create_func create = { NULL, NULL, NULL };
	create.create_func = &modbus_constructor;

	godot_instance_destroy_func destroy = { NULL, NULL, NULL };
	destroy.destroy_func = &modbus_destructor;

	nativescript_api->godot_nativescript_register_class(p_handle, "ModbusServerNative", "Reference",
			create, destroy);

	godot_method_attributes attributes = { GODOT_METHOD_RPC_MODE_DISABLED };

	godot_instance_method get_holding_register = { NULL, NULL, NULL };
	get_holding_register.method = &modbus_get_holding_register;

	nativescript_api->godot_nativescript_register_method(p_handle, "ModbusServerNative", "get_holding_register",
			attributes, get_holding_register);
	
	godot_instance_method set_holding_register = { NULL, NULL, NULL };
	set_holding_register.method = &modbus_set_holding_register;

	nativescript_api->godot_nativescript_register_method(p_handle, "ModbusServerNative", "set_holding_register",
			attributes, set_holding_register);
	
	godot_instance_method start_server = { NULL, NULL, NULL };
	start_server.method = &modbus_start_server;

	nativescript_api->godot_nativescript_register_method(p_handle, "ModbusServerNative", "start_server",
			attributes, start_server);

	godot_instance_method set_coil = { NULL, NULL, NULL };
	set_coil.method = &modbus_set_coil;

	nativescript_api->godot_nativescript_register_method(p_handle, "ModbusServerNative", "set_coil",
			attributes, set_coil);
	
	godot_instance_method get_coil = { NULL, NULL, NULL };
	get_coil.method = &modbus_get_coil;

	nativescript_api->godot_nativescript_register_method(p_handle, "ModbusServerNative", "get_coil",
			attributes, get_coil);
}

